/*
 * Copyright (C) 2023 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */
#include <stdio.h>
#include <unistd.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_errno.h"
#include "iot_gpio.h"
#include "iot_pwm.h"
#include "iot_i2c.h"
#include "hi_io.h"
#include "hi_i2c.h"
#include "hi_errno.h"

#include "ssd1306.h"
#include "icm.h"

unsigned char fifo_data[17];

#define     CLK_160M                160000000

uint16_t acc_data_X;
uint16_t acc_data_Y;
uint16_t acc_data_Z;

uint16_t acc_data_X0;
uint16_t acc_data_Y0;
uint16_t acc_data_Z0;

uint16_t acc_data_X1;
uint16_t acc_data_Y1;
uint16_t acc_data_Z1;

uint16_t gyro_data_X;
uint16_t gyro_data_Y;
uint16_t gyro_data_Z;

uint16_t gyro_data_X0;
uint16_t gyro_data_Y0;
uint16_t gyro_data_Z0;

uint16_t gyro_data_X1;
uint16_t gyro_data_Y1;
uint16_t gyro_data_Z1;

void SixAxisTask(void)
{
    unsigned int IR_i;
    unsigned short data = 0; 
    char str[50];
    uint32_t status = 0;
	uint8_t ReadFIFODataCmd [] = {FIFO_DATA_REG};

    icm_initialize();

	status = IoTI2cWrite(WIFI_IOT_I2C_IDX_0, device_address, ReadFIFODataCmd, sizeof(ReadFIFODataCmd));
	osDelay(10);

	if (status != IOT_SUCCESS) {
		printf("===== Error: ICM40607 ReadFIFOData I2C write status = 0x%x! =====\r\n", status);
        snprintf(str, sizeof(str), "6 axis NG!");
	} else {
        snprintf(str, sizeof(str), "6 axis OK!");
    }

    ssd1306_Fill(Black);
    ssd1306_SetCursor(10, 10);
    ssd1306_DrawString("SixAxisTest", Font_7x10, White);
    ssd1306_SetCursor(10, 10+10+10);
    ssd1306_DrawString(str, Font_11x18, White);
    ssd1306_UpdateScreen();

	IoTI2cRead(WIFI_IOT_I2C_IDX_0, device_address, fifo_data, sizeof(fifo_data));

	// printf("fifo_data[0]:%d\r\n",fifo_data[0]);		
    acc_data_X1 = fifo_data[1];
    acc_data_X0 = fifo_data[2];
    acc_data_X = (acc_data_X1<<8) | acc_data_X0;

    acc_data_Y1 = fifo_data[3];
    acc_data_Y0 = fifo_data[4];
    acc_data_Y = (acc_data_Y1<<8) | acc_data_Y0;

    acc_data_Z1 = fifo_data[5];
    acc_data_Z0 = fifo_data[6];
    acc_data_Z = (acc_data_Z1<<8) | acc_data_Z0;

    gyro_data_X1 = fifo_data[7];
    gyro_data_X0 = fifo_data[8];
    gyro_data_X = (gyro_data_X1<<8) | gyro_data_X0;

    gyro_data_Y1 = fifo_data[9];
    gyro_data_Y0 = fifo_data[10];
    gyro_data_Y = (gyro_data_Y1<<8) | gyro_data_Y0;

    gyro_data_Z1 = fifo_data[11];
    gyro_data_Z0 = fifo_data[12];
    gyro_data_Z = (gyro_data_Z1<<8) | gyro_data_Z0;

    usleep(100000); 

    printf("acc_data_X = %d\r\n", (unsigned int)acc_data_X);
    printf("acc_data_Y = %d\r\n", (unsigned int)acc_data_Y);
    printf("acc_data_Z = %d\r\n", (unsigned int)acc_data_Z);

    printf("gyro_data_X = %d\r\n", (unsigned int)gyro_data_X);
    printf("gyro_data_Y = %d\r\n", (unsigned int)gyro_data_Y);
    printf("gyro_data_Z = %d\r\n", (unsigned int)gyro_data_Z);
}


void SixAxisTestEntry(void)
{
#if 1
    SixAxisTask();
#else
    //osPriorityAboveNormal[32], osPriorityNormal[24]
    //{.name, .attr_bits, .cb_mem, .cb_size, .stack_mem, .stack_size, .priority, .tz_module, .reserved}
    osThreadAttr_t attr = {"SixAxisTask", 0, NULL, 0, NULL, 1024*10, 24, 0, 0};
    osThreadId_t taskId = osThreadNew((osThreadFunc_t)SixAxisTask, NULL, &attr);
    if (taskId == NULL) {
        printf("[SixAxisTestEntry] Falied to create %s!\n", attr.name);
    }
#endif
}